import math
from soccerbots.core.structs import *
import random

def torad(angle):
    return (angle*math.pi)/180

def togra(angle):
    return int( ((angle*180)/math.pi)%360 )

class AI:
    def __init__(self):
        self.ERROR = False

    def distance(self, posa, posb):
        return math.sqrt(math.pow(posa[0]-posb[0],2)+math.pow(posa[0]-posb[0],2))

    def distance_eu(self,row1,col1,row2,col2):
        return(math.sqrt(math.pow(row1-row2,2) + math.pow(col1-col2,2)))
    
    def getAng(self, row1, col1, row2, col2):
        drow = row1 - row2
        dcol = col1 - col2
        return(math.atan(dcol/drow)*(180/math.pi))

    def setAngVel(self, orientation,angle):
        if (orientation - angle) == 0:
            return(self.mav)
        else:
            return(abs(orientation-angle))

    def setLinVel(self, row1,col1,row2,col2):
        dist = self.distance_eu(row1,col1,row2,col2)
        if (dist > self.mlv):
            return (self.mlv)
        else:
            return (dist)

    def moveToSimple(self,robot,robot_row,robot_col,orientation,go_row,go_col):
        dist = self.distance_eu(robot_row,robot_col,go_row,go_col)
        angle = self.getAng(robot_row,robot_col,go_row,go_col)
        if (orientation == angle) :
            if (distance == 0):
                return (int(0), int(0))
            else:
                return (int(self.setLinVel(robot_row,robot_col,go_row,go_col)), int(0))
        else:
                return (int(0), int(self.setAngVel(orientation,angle)))

    def moveTo(self, robot, position, direction):
        dx =position[0] - robot.position[0]
        dy =position[1] - robot.position[1]
        hipo = math.sqrt(math.pow(dx,2)+math.pow(dy,2))
        ang = togra(math.atan(dy/max(dx,1)))
        print "HOLAAAAAAAAAAAAAAAAA ",ang,robot.direction
        max_a = self.mav
        if self.ball.linearVel == 0:
            lv = 0
            av = 0
            if position[0] > robot.position[0]:
                av = -max_a
                lv = 0
                if robot.direction < 30 or robot.direction > 340:
                    av = 0
                    lv = self.mlv
            elif position[0] <  robot.position[0]:
                av = max_a
                lv = 0
                if robot.direction > 140 and robot.direction > 220:
                    av = 0
                    lv = self.mlv
            if lv != 0:
                if position[1] > robot.position[1]:
                    av = -max_a
                    lv = 0
                    if robot.direction < 110 or robot.direction > 70:
                        av = 0
                        lv = self.mlv
                elif position[1] <  robot.position[1]:
                    av = max_a
                    lv = 0
                    if robot.direction > 240 and robot.direction > 290:
                        av = 0
                        lv = self.mlv
            if hipo <= 300:
                lv = self.mlv
                av = 0
        else:
            a=ang
            #a = direction - robot.direction
            if a<0:
                av = -max_a
            else:
                av = max_a

            lv = self.mlv

            dx =position[0] - robot.position[0]
            dy =position[1] - robot.position[1]
            ang = togra(math.atan(dy/max(dx,1)))
            if dx < 0: ang = (ang+180)%360
            if (abs((robot.direction - ang)%360) == 180):
                lv *= -1
            elif(abs((robot.direction - ang)%360) == 0):
                lv *= 1 
            elif abs((robot.direction - ang)%360) > 90 and abs((robot.direction - ang)%360) < 270:
                lv *= -1
            elif abs((robot.direction - ang)%360) < 90 and abs((robot.direction - ang)%360) > 270:
                lv = abs(lv)
                

        return int(lv),int(av)

    def setMaxLinearVel(self, vel):
        self.mlv = vel

    def setMaxAngularVel(self, vel):
        self.mav = vel

    def acotar(self, val, max):
        if val > max:
            return max
        elif val > 0:
            return val
        else:
            return 0

    def processState(self, state, team=1):
        teams = [state.teamA,state.teamB]
        myteam = teams[team-1]
        theirteam = teams[2-team]
        
        if myteam['goalkeeper'].position[0] < 0:
            side = -1
        else:
            side = 1

        mov = TeamMovement()
        k = TeamMovement.keyset[0]
        mov[k] = PlayerMovement()
        porter = myteam[k]
        camp = porter.position[0]/abs(porter.position[0])
        vel = max(porter.linearVel,1)/abs(max(porter.linearVel,1))
        if camp < 0:
            if (porter.position[1]-900)>220:
                mov[k].linearVelocity = self.mlv
            elif (porter.position[1]-900)<-220:
                mov[k].linearVelocity = -self.mlv
            else:
                mov[k].linearVelocity = porter.linearVel
        else:
            if (porter.position[1]-900)>220:
                mov[k].linearVelocity = -self.mlv
            elif (porter.position[1]-900)<-220:
                mov[k].linearVelocity = self.mlv
            else:
                mov[k].linearVelocity = porter.linearVel
        if abs(porter.linearVel) < 50:
            mov[k].linearVelocity = self.mlv
        mov[k].angVelocity = 0

        for k in TeamMovement.keyset[1:]:
            #lv,av = self.moveTo(myteam[k],theirteam[k].position,theirteam[k].direction)
            #def moveToSimple(self,robot,robot_row,robot_col,orientation,go_row,go_col):
            self.ball = state.ball
            lv,av = self.moveTo(myteam[k],state.ball.position,state.ball.direction)
            #lv,av = self.moveToSimple(myteam[k],myteam[k].position[0],myteam[k].position[1],myteam[k].direction, state.ball.position[0],state.ball.position[1])
            mov[k] = PlayerMovement()
            mov[k].linearVelocity = lv
            mov[k].angVelocity = av

        ''' 
        k = TeamMovement.keyset[-1]
        lv,av = self.moveTo(myteam[k],state.ball.position,state.ball.direction)
        mov[k] = PlayerMovement()
        mov[k].linearVelocity = lv
        mov[k].angVelocity = av
        '''

        print mov
        return mov
